#20250108 at1520  7.4.3调用接口进行单点导航
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
import rclpy
from rclpy.duration import Duration

def main():
    rclpy.init()
    navigator = BasicNavigator() #节点
    navigator.waitUntilNav2Active() #等待导航可用
    

    goal_pose = PoseStamped() #消息接口：：ros2 action send_goal  /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose:{header: {frame_id: map} ,pose: { position: {x: 2.0, y: 2.0}}}}" --feedback 
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 2.0
    goal_pose.pose.position.y = 1.0
    goal_pose.pose.orientation.w = 1.0

    #发送目标位置，接受反馈：
    navigator.goToPose(goal_pose)
    
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
        navigator.get_logger().info(
            f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达'
        )

        # 超时自动取消
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelTask()

    
    
    # navigator.setInitialPose(goal_pose)#方法
    # 最终结果判断
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        navigator.get_logger().info('导航结果：成功')
    elif result == TaskResult.CANCELED:
        navigator.get_logger().warn('导航结果：被取消')
    elif result == TaskResult.FAILED:
        navigator.get_logger().error('导航结果：失败')
    else:
        navigator.get_logger().error('导航结果：返回状态无效')



    #rclpy.spin(navigator)
    #rclpy.shutdown()

if __name__ == '__main__':
    main()
